#include "scratch_detection.h"

ScratchDetection::ScratchDetection() {

  cloud_src = PointCloudT::Ptr(new PointCloudT);
  scratch_cloud = PointCloudT::Ptr(new PointCloudT);

}

ScratchDetection::~ScratchDetection() {}

void ScratchDetection::setInputCloud(PointCloudT::Ptr &cloud_in) {
  pcl::copyPointCloud(*cloud_in, *cloud_src);
  cout << "src cloud has: " << cloud_src->points.size() << " data points."
       << endl;
}

int ScratchDetection::compute() {

    //设置默认输入参数
    //  float limit_min = 2;                  //直通滤波下边界
    //  float limit_max = 100;                //直通滤波上边界
    double planDistanceThreshold1 = 0.1; //平面拟合1距离阈值
    double planDistanceThreshold2 = 0.8;   //平面拟合1距离阈值
    double ClusterTolerance = 1;
//    pcl::console::parse_argument(argc, argv, "--ct", ClusterTolerance);
//    // pcl::console::parse_argument(argc, argv, "-bc", bool_cuting);
//    //  pcl::console::parse_argument(argc, argv, "-lmin", limit_min);
//    //  pcl::console::parse_argument(argc, argv, "-lmax", limit_max);
//    pcl::console::parse_argument(argc, argv, "-dt", planDistanceThreshold1);

    // objects needed
    pcl::VoxelGrid<PointT> voxelFilter;
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    pcl::ExtractIndices<PointT> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
    pcl::PassThrough<PointT> pass;
    // Datasets
//    PointCloudT::Ptr cloud_src(new PointCloudT); //输入lidar点云
    PointCloudT::Ptr cloud_downsample(new PointCloudT);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(
        new pcl::PointCloud<pcl::Normal>);

    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
    PointCloudT::Ptr cloud_plane(new PointCloudT);
    PointCloudT::Ptr cloud_rest(new PointCloudT);
    PointCloudT::Ptr cloud_trans(new PointCloudT);
    pcl::PCDReader reader;
    pcl::PCDWriter writer;

//    //读入数据
//    string data_addr = argv[1];
//    reader.read(data_addr + argv[2], *cloud_src);

//    cout << "src cloud has: " << cloud_src->points.size() << " data points."
//         << endl;

    //法线估计
    // Estimate point normals
    ne.setSearchMethod(tree);
    ne.setInputCloud(cloud_src);
    ne.setKSearch(30);
    ne.compute(*cloud_normals);

    // Create the segmentation object for the planar model and set all the
    // parameters
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight(0.1);
    seg.setMethodType(pcl::SAC_RANSAC);
    // seg.setMaxIterations(100);
    seg.setDistanceThreshold(planDistanceThreshold1);
    seg.setInputCloud(cloud_src);
    seg.setInputNormals(cloud_normals);
    // Obtain the plane inliers and coefficients
    seg.segment(*inliers_plane, *coefficients_plane);

    if (inliers_plane->indices.size() == 0) {
        PCL_ERROR("Could not estimate a planar model for the given dataset.");
        return (-1);
    }

    cout << "plane inliers has: " << inliers_plane->indices.size()
         << " data points." << endl;
    cout << "Plane coefficients: " << *coefficients_plane << std::endl;

    //将拟合平面旋转平移到xoy面
    Eigen::Affine3f trans_matrix = Eigen::Affine3f::Identity();

    Trans2Plane(coefficients_plane, trans_matrix);

    pcl::transformPointCloud(*cloud_src, *cloud_trans, trans_matrix);
//    writer.write(data_addr + "Trans1.pcd", *cloud_trans, false);

    // Extract the planar inliers from the input cloud
    extract.setInputCloud(cloud_trans);
    extract.setIndices(inliers_plane);
    extract.setNegative(false);
    extract.filter(*cloud_plane);
    cout << "cloud plane has: " << cloud_plane->points.size() << " data points."
         << std::endl;
//    writer.write(data_addr + "plane.pcd", *cloud_plane, false);

    // Remove the planar inliers, extract the rest
    extract.setNegative(true);
    extract.filter(*cloud_rest);
    cout << "cloud rest has: " << cloud_rest->points.size() << " data points."
         << std::endl;
//    writer.write(data_addr + "rest.pcd", *cloud_rest, false);

    //增大ｚ坐标差异
    PointCloudT::Ptr cloud_increaseZ(new PointCloudT);
    float increaseK = 10;
    IncreaseZ(cloud_trans, cloud_increaseZ, increaseK);
//    writer.write(data_addr + "cloud_upZ.pcd", *cloud_increaseZ, false);

    //第二次平面拟合
    //法线估计
    // Estimate point normals
    pcl::search::KdTree<PointT>::Ptr tree1(new pcl::search::KdTree<PointT>());
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1(
        new pcl::PointCloud<pcl::Normal>);
    ne.setSearchMethod(tree1);
    ne.setInputCloud(cloud_increaseZ);
    ne.setKSearch(30);
    ne.compute(*cloud_normals1);

    // Create the segmentation object for the planar model and set all the
    // parameters
    pcl::ModelCoefficients::Ptr coefficients_plane1(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_plane1(new pcl::PointIndices);
    seg.setNormalDistanceWeight(0.1);
    // seg.setMaxIterations(100);
    seg.setDistanceThreshold(planDistanceThreshold2);
    seg.setInputCloud(cloud_increaseZ);
    seg.setInputNormals(cloud_normals1);
    // Obtain the plane inliers and coefficients
    seg.segment(*inliers_plane1, *coefficients_plane1);

    if (inliers_plane1->indices.size() == 0) {
        PCL_ERROR("Could not estimate a planar model for the given dataset.");
        return (-1);
    }

    cout << "plane1 inliers has: " << inliers_plane1->indices.size()
         << " data points." << endl;
    cout << "Plane1 coefficients: " << *coefficients_plane1 << std::endl;

    // Extract the planar inliers from the input cloud
    extract.setInputCloud(cloud_increaseZ);
    extract.setIndices(inliers_plane1);
    extract.setNegative(false);
    extract.filter(*cloud_plane);
    cout << "cloud plane has: " << cloud_plane->points.size() << " data points."
         << std::endl;
//    writer.write(data_addr + "plan1.pcd", *cloud_plane, false);

    // Remove the planar inliers, extract the rest
    extract.setNegative(true);
    extract.filter(*cloud_rest);
    cout << "cloud rest has: " << cloud_rest->points.size() << " data points."
         << std::endl;
//    writer.write(data_addr + "rest1.pcd", *cloud_rest, false);


    PointCloudT::Ptr cloud_filtered(new PointCloudT);
    //直通滤波
    // Build a passthrough filter to remove spurious NaNs
    pass.setInputCloud(cloud_increaseZ);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(-100, -0.5);
    pass.filter(*cloud_filtered);
//    writer.write(data_addr + "restPass.pcd", *cloud_filtered, false);

    //  //sor滤波移除离群点
    //  // Create the filtering object
    //  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    //  sor.setInputCloud (cloud_rest);
    //  sor.setMeanK (10);
    //  sor.setStddevMulThresh (1.0);
    //  sor.filter (*cloud_filtered);
    //  writer.write(data_addr + "sor.pcd", *cloud_filtered, false);
    //直线拟合

    //计算深度
    scratch_depth = compute_depth(cloud_filtered)/increaseK;
    cout << "depth: " << scratch_depth <<endl;


    //投影
    PointCloudT::Ptr cloud_proj(new PointCloudT);
    proj2z0(cloud_filtered,cloud_proj,-100);
//    writer.write(data_addr + "proj.pcd", *cloud_proj, false);

    //直线拟合
    pcl::ModelCoefficients::Ptr line_coeff1(new pcl::ModelCoefficients);

    pcl::PointIndices::Ptr line_inliers1(new pcl::PointIndices);


    // Create the segmentation object
    pcl::SACSegmentation<PointT> lineSeg;
    lineSeg.setOptimizeCoefficients(true);
    lineSeg.setModelType(pcl::SACMODEL_LINE);
    lineSeg.setMethodType(pcl::SAC_RANSAC);
    lineSeg.setDistanceThreshold(1); //距离阈值1mm
    // one
    lineSeg.setInputCloud(cloud_proj);
    lineSeg.segment(*line_inliers1, *line_coeff1);
    //cout << "line coefficients1: " << *line_coeff1 << std::endl;

    //投影平面上的划痕点
    PointCloudT::Ptr line1(new PointCloudT);

    extract.setInputCloud(cloud_proj);
    extract.setIndices(line_inliers1);
    extract.setNegative(false); // false,内点;true，外点
    extract.filter(*line1);
//    writer.write(data_addr + "line.pcd", *line1, false);

    //提取最后检测出的划痕点
    PointCloudT::Ptr scratchUpz(new PointCloudT);

    extract.setInputCloud(cloud_filtered);
    extract.setIndices(line_inliers1);
    extract.setNegative(false); // false,内点;true，外点
    extract.filter(*scratchUpz);
//    writer.write(data_addr + "scratchUpZ.pcd", *scratchUpz, false);
    //缩小还原ｚ坐标
    IncreaseZ(scratchUpz,scratch_cloud,1/increaseK);
//    writer.write(data_addr + "scratch.pcd", *scratch, false);
    //旋转平移到原始状态
    pcl::transformPointCloud(*scratch_cloud, *scratch_cloud, trans_matrix.inverse());

    //将直线方向旋转至(0,1,0)并通过坐标原点,便于计算长度，宽度
    Eigen::Affine3f trans_matrix2  = Eigen::Affine3f::Identity();
    Trans2line(line_coeff1,trans_matrix2);
    PointCloudT::Ptr lineT(new PointCloudT);
    pcl::transformPointCloud(*line1,*lineT,trans_matrix2);

//    writer.write(data_addr + "lineT.pcd", *lineT, false);

    pcl::transformPointCloud(*cloud_trans, *cloud_trans, trans_matrix2);
//    writer.write(data_addr + "Trans2.pcd", *cloud_trans, false);
    //计算宽度
    scratch_width =  compute_width(lineT);
    cout << "width: " << scratch_width << endl;

    //计算长度
    scratch_length = compute_length(lineT);
    cout << "length: " << scratch_length << endl;

    return 0;
}

void ScratchDetection::Trans2line(pcl::ModelCoefficients::Ptr &line_coeff,
                Eigen::Affine3f &trans_matrix) {
    //cout << "transform line cloud to (0,1,0) ......" << endl;

    Eigen::Vector3f vec1 = Eigen::Vector3f::Zero();
    vec1 << line_coeff->values[3], line_coeff->values[4], line_coeff->values[5];
    Eigen::Vector3f vec2(0, 1, 0);

    // Eigen::Matrix3d rot_matrix = Eigen::Matrix3d::Identity();
    //求两向量之间的旋转向量
    //夹角
    float theta = acos(vec1.dot(vec2) / vec1.norm());
    //旋转轴
    Eigen::Vector3f vec3 = vec1.cross(vec2);
    vec3 = vec3 / vec3.norm();
    //旋转向量
    Eigen::AngleAxisf rot_vec(theta, vec3);
    //  cout << "rot vec : " << endl;
    //  cout << rot_vec.matrix() << endl;

    //  //旋转矩阵
    //  rot_matrix = rot_vec.toRotationMatrix();
    //  cout << "rot matrix: " << endl;

    //  cout << rot_matrix << endl;

    //平移向量
    Eigen::Vector3f trans = Eigen::Vector3f::Zero();
    trans << -line_coeff->values[0], -line_coeff->values[0], -line_coeff->values[0];

    //  cout << "trans vec:" << endl;
    //  cout << trans.matrix() << endl;

    trans_matrix.rotate(rot_vec);
    trans_matrix.pretranslate(trans);

    cout << "trans to line matrix ::" << endl;
    cout << trans_matrix.matrix() << endl;
    cout << "transform completed." << endl;
}
void ScratchDetection::Trans2Plane(pcl::ModelCoefficients::Ptr &plan_coeff,
                 Eigen::Affine3f &trans_matrix) {
    //cout << "transform cloud to xoy plan......" << endl;

    Eigen::Vector3f vec1 = Eigen::Vector3f::Zero();
    vec1 << plan_coeff->values[0], plan_coeff->values[1], plan_coeff->values[2];
    Eigen::Vector3f vec2(0, 0, 1);

    // Eigen::Matrix3d rot_matrix = Eigen::Matrix3d::Identity();
    //求两向量之间的旋转向量
    //夹角
    float theta = acos(vec1.dot(vec2) / vec1.norm());
    //旋转轴
    Eigen::Vector3f vec3 = vec1.cross(vec2);
    vec3 = vec3 / vec3.norm();
    //旋转向量
    Eigen::AngleAxisf rot_vec(theta, vec3);
    //  cout << "rot vec : " << endl;
    //  cout << rot_vec.matrix() << endl;

    //  //旋转矩阵
    //  rot_matrix = rot_vec.toRotationMatrix();
    //  cout << "rot matrix: " << endl;

    //  cout << rot_matrix << endl;

    //平移向量
    Eigen::Vector3f trans = Eigen::Vector3f::Zero();
    trans << 0, 0, (plan_coeff->values[3]);

    //  cout << "trans vec:" << endl;
    //  cout << trans.matrix() << endl;

    trans_matrix.rotate(rot_vec);
    trans_matrix.pretranslate(trans);

    cout << "trans to plane matrix ::" << endl;
    cout << trans_matrix.matrix() << endl;
    cout << "transform completed." << endl;
}
//计算点云到平面的最远距离
float ScratchDetection::compute_depth(PointCloudT::Ptr &cloud_in) {
    Eigen::Vector4f planeCoeff = Eigen::Vector4f::Zero();

    planeCoeff << 0, 0, 1, 0; // z=0平面

    vector<float> distance;
    int pointNum = static_cast<int>(cloud_in->points.size());
    for (int i = 0; i < pointNum; i++) {
        Eigen::Vector4f point = Eigen::Vector4f::Zero();
        point << cloud_in->points[i].x, cloud_in->points[i].y,
            cloud_in->points[i].z, 1;
        distance.push_back(fabs(point.dot(planeCoeff)));
    }
    //排序
    sort(distance.begin(), distance.end());
    //取前最大值为最远距离
    float maxDistance = *(distance.end()-1);
    return (maxDistance);
}
float ScratchDetection::compute_width(PointCloudT::Ptr &cloud_in) {

    vector<float> width_vec;
    int pointNum = static_cast<int>(cloud_in->points.size());
    for (int i = 0; i < pointNum; i++) {
        width_vec.push_back(cloud_in->points[i].x);
    }
    //排序
    sort(width_vec.begin(), width_vec.end());
    //取前最大值为最远距离的估计
    float maxWidth = fabs(*(width_vec.end()-1)-*(width_vec.begin()));
    return (maxWidth);
}
float ScratchDetection::compute_length(PointCloudT::Ptr &cloud_in) {

    vector<float> length_vec;
    int pointNum = static_cast<int>(cloud_in->points.size());
    for (int i = 0; i < pointNum; i++) {
        length_vec.push_back(cloud_in->points[i].y);
    }
    //排序
    sort(length_vec.begin(), length_vec.end());
    //取最大最小值之差为长度的估计值
    float maxLength = *(length_vec.end()-1)-*(length_vec.begin());
    return (maxLength);
}
void ScratchDetection::proj2z0(PointCloudT::Ptr &cloud_in, PointCloudT::Ptr &cloud_out,float zlimit) {
    int pointNum = static_cast<int>(cloud_in->points.size());
    PointT points;
    points.z = 0;
    for (int i = 0; i < pointNum; i++) {
        //只保留靠近边沿的点
        if (cloud_in->points[i].z < zlimit)
            continue;
        points.x = cloud_in->points[i].x;
        points.y = cloud_in->points[i].y;
        cloud_out->push_back(points);
    }
}
void ScratchDetection::IncreaseZ(PointCloudT::Ptr &cloud_in, PointCloudT::Ptr &cloud_out,
               float increaseK) {
    int pointNum = static_cast<int>(cloud_in->points.size());
    PointT points;

    for (int i = 0; i < pointNum; i++) {
        //        //只保留靠近边沿的点
        //        if (cloud_in->points[i].z < -0.2)
        //            continue;
        points.x = cloud_in->points[i].x;
        points.y = cloud_in->points[i].y;
        points.z = increaseK * cloud_in->points[i].z;
        cloud_out->push_back(points);
    }
}
